#include "./pick_service.h"
#include "../master_conn.h"
#include "../../driver/ice/parts.h"
#include "../../conn/proto.h"
#include "../../driver/ice/stir_motor.h"

namespace service {

using namespace utils;
using namespace driver;
using namespace conn;

constexpr uint8_t ICE_LEFT = 0;
constexpr uint8_t ICE_RIGHT = 1;

Error ice_motor_ctrl(uint8_t dir) {
    RespInfo info;
    Message msg;
    Error err = masterConn->async_request(
        info, 
        addr::HEAT, 
        proto::REQ_HEAT_ICE_MOTOR_CTRL, 
        dir
        );
    if (err) {
        goto finish;
    }
    err = masterConn->wait_for(info, msg);
    if (err) {
        goto finish;
    }
    err = msg.parse_ec();
finish:
    return err;
}

Error drop_ice(uint8_t sec) {
    Error err = ice_motor_ctrl(ICE_LEFT);
    if (err) {
        goto finish;
    }
    stirMotor->open();
    parts->open_drop_ice();
    osDelay(sec * 1000);
    parts->close_drop_ice();
    stirMotor->close_once();
    osDelay(1000);
    err = ice_motor_ctrl(ICE_RIGHT);
finish:
    return err;
}

}